#pragma once

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include <general_interface/action/switch_mode.hpp>
#include <general_interface/msg/work_mode.hpp>

#include <core/task.hpp>
#include <map>

class Core : public rclcpp::Node
{
    using SWMode = general_interface::action::SwitchMode;
    using WorkMode = general_interface::msg::WorkMode;

  private:
    std::shared_ptr<Task> current_task;
    std::map<decltype(WorkMode::mode), std::shared_ptr<Task>> tasks;
    rclcpp_action::Server<SWMode>::SharedPtr switch_mode_server;
    std::shared_ptr<Context> context;

  public:
    explicit Core();
    void Initialize();
    rclcpp_action::GoalResponse OnGoal(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const SWMode::Goal> goal);
    rclcpp_action::CancelResponse OnCancel(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<SWMode>> handle);
    void OnAccepted(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<SWMode>> handle);
};